// ESP32-S3-CAM - 主控制器
#include <IRremote.h>
#include <ESP32Servo.h>

// 红外接收配置
#define VS1838B_PIN 13
IRrecv irrecv(VS1838B_PIN);
decode_results results;

// 自定义红外编码（与Arduino匹配）
const unsigned long TARGET_IR_CODE = 0x12345678;

// 红外对管配置 - 分时复用
#define IR_EMIT_1   14  // 左发射管
#define IR_EMIT_2   15  // 前发射管  
#define IR_EMIT_3   16  // 右发射管
#define IR_RECEIVE_PIN 4  // 共用接收管（PT908-7C）

// 电机控制引脚（假设使用L298N驱动）
#define MOTOR_A_IN1 5
#define MOTOR_A_IN2 6
#define MOTOR_B_IN1 7
#define MOTOR_B_IN2 8

// 运动状态
enum MovementState {
  SEARCHING,      // 搜索红外信号
  NAVIGATING,     // 导航向信号源
  AVOIDING,       // 避障
  ARRIVED         // 到达目标
};

MovementState currentState = SEARCHING;

// 传感器数据
int sensorValues[3] = {0};      // 左、前、右传感器值
bool obstacles[3] = {false};    // 左、前、右障碍物状态
int irSignalStrength = 0;       // 红外信号强度
unsigned long lastIrTime = 0;   // 最后收到红外信号时间
int signalDirection = 0;        // 信号方向估计 (-1:左, 0:前, 1:右)

// 校准参数
int thresholds[3] = {500, 500, 500};  // 障碍物检测阈值

void setup() {
  Serial.begin(115200);
  
  // 初始化红外接收
  irrecv.enableIRIn();
  Serial.println("VS1838B红外接收初始化完成");
  
  // 初始化红外发射管引脚
  pinMode(IR_EMIT_1, OUTPUT);
  pinMode(IR_EMIT_2, OUTPUT);
  pinMode(IR_EMIT_3, OUTPUT);
  digitalWrite(IR_EMIT_1, LOW);
  digitalWrite(IR_EMIT_2, LOW);
  digitalWrite(IR_EMIT_3, LOW);
  
  // 初始化电机控制引脚
  pinMode(MOTOR_A_IN1, OUTPUT);
  pinMode(MOTOR_A_IN2, OUTPUT);
  pinMode(MOTOR_B_IN1, OUTPUT);
  pinMode(MOTOR_B_IN2, OUTPUT);
  
  // 停止电机
  stopMotors();
  
  Serial.println("ESP32-S3-CAM导航系统初始化完成");
  Serial.println("开始搜索红外信号源...");
}

void loop() {
  // 1. 读取所有传感器
  readAllSensors();
  
  // 2. 处理红外信号
  processIrSignal();
  
  // 3. 状态机决策
  stateMachine();
  
  // 4. 显示状态信息
  displayStatus();
  
  delay(50); // 主循环频率约20Hz
}

// 读取三路红外传感器（分时复用）
void readAllSensors() {
  for (int i = 0; i < 3; i++) {
    sensorValues[i] = readSensor(i);
    obstacles[i] = (sensorValues[i] < thresholds[i]);
    delay(2); // 传感器间短暂间隔
  }
}

// 读取指定传感器
int readSensor(int sensorIndex) {
  // 关闭所有发射管
  digitalWrite(IR_EMIT_1, LOW);
  digitalWrite(IR_EMIT_2, LOW);
  digitalWrite(IR_EMIT_3, LOW);
  
  delayMicroseconds(100);
  
  // 打开指定发射管
  switch(sensorIndex) {
    case 0: digitalWrite(IR_EMIT_1, HIGH); break; // 左
    case 1: digitalWrite(IR_EMIT_2, HIGH); break; // 前
    case 2: digitalWrite(IR_EMIT_3, HIGH); break; // 右
  }
  
  delayMicroseconds(500);
  
  // 读取接收管值
  int value = analogRead(IR_RECEIVE_PIN);
  
  // 关闭当前发射管
  switch(sensorIndex) {
    case 0: digitalWrite(IR_EMIT_1, LOW); break;
    case 1: digitalWrite(IR_EMIT_2, LOW); break;
    case 2: digitalWrite(IR_EMIT_3, LOW); break;
  }
  
  return value;
}

// 处理VS1838B接收的红外信号
void processIrSignal() {
  if (irrecv.decode(&results)) {
    if (results.value == TARGET_IR_CODE) {
      // 收到目标信号
      lastIrTime = millis();
      irSignalStrength = calculateSignalStrength();
      
      Serial.printf("收到目标红外信号! 强度: %d\n", irSignalStrength);
      
      // 简单估计信号方向（基于信号强度变化）
      estimateSignalDirection();
    }
    irrecv.resume(); // 准备接收下一个信号
  }
  
  // 检查信号是否丢失（2秒内未收到信号）
  if (millis() - lastIrTime > 2000) {
    irSignalStrength = 0;
    currentState = SEARCHING;
  }
}

// 计算信号强度（简化版）
int calculateSignalStrength() {
  // 基于接收到的信号质量估计强度
  // 实际应用中可能需要更复杂的算法
  return random(50, 100); // 模拟信号强度
}

// 估计信号方向（简化版）
void estimateSignalDirection() {
  // 在实际应用中，这里应该使用多个接收器或旋转天线
  // 这里使用随机模拟
  signalDirection = random(-1, 2); // -1, 0, 1
}

// 主状态机
void stateMachine() {
  switch (currentState) {
    case SEARCHING:
      handleSearchingState();
      break;
    case NAVIGATING:
      handleNavigatingState();
      break;
    case AVOIDING:
      handleAvoidingState();
      break;
    case ARRIVED:
      handleArrivedState();
      break;
  }
}

// 搜索状态处理
void handleSearchingState() {
  if (irSignalStrength > 0) {
    Serial.println("发现红外信号，切换到导航状态");
    currentState = NAVIGATING;
    return;
  }
  
  // 未发现信号，原地旋转搜索
  Serial.println("旋转搜索红外信号...");
  rotateSlowly();
}

// 导航状态处理
void handleNavigatingState() {
  // 检查前方障碍物
  if (obstacles[1]) { // 前方有障碍
    Serial.println("前方障碍物，切换到避障状态");
    currentState = AVOIDING;
    return;
  }
  
  // 根据信号方向导航
  if (signalDirection == -1) {
    // 信号在左侧，轻微左转前进
    moveForwardLeft();
    Serial.println("导航: 向左前进");
  } else if (signalDirection == 1) {
    // 信号在右侧，轻微右转前进
    moveForwardRight();
    Serial.println("导航: 向右前进");
  } else {
    // 信号在前方，直行
    moveForward();
    Serial.println("导航: 直行前进");
  }
  
  // 检查是否到达目标（信号强度足够强）
  if (irSignalStrength > 90) {
    Serial.println("接近目标，切换到到达状态");
    currentState = ARRIVED;
  }
}

// 避障状态处理
void handleAvoidingState() {
  Serial.println("执行避障策略");
  
  if (!obstacles[0] && !obstacles[2]) {
    // 左右都畅通，根据信号方向选择
    if (signalDirection <= 0) {
      turnLeft();
    } else {
      turnRight();
    }
  } else if (!obstacles[0]) {
    // 左侧畅通
    turnLeft();
  } else if (!obstacles[2]) {
    // 右侧畅通
    turnRight();
  } else {
    // 左右都有障碍，后退
    moveBackward(1000);
    turnLeft(1500);
  }
  
  // 短暂避障后返回导航
  delay(800);
  currentState = NAVIGATING;
}

// 到达状态处理
void handleArrivedState() {
  stopMotors();
  Serial.println("🎉 成功到达红外信号源位置!");
  
  // 闪烁LED表示到达
  digitalWrite(LED_BUILTIN, HIGH);
  delay(500);
  digitalWrite(LED_BUILTIN, LOW);
  delay(500);
}

// 电机控制函数
void stopMotors() {
  digitalWrite(MOTOR_A_IN1, LOW);
  digitalWrite(MOTOR_A_IN2, LOW);
  digitalWrite(MOTOR_B_IN1, LOW);
  digitalWrite(MOTOR_B_IN2, LOW);
}

void moveForward() {
  digitalWrite(MOTOR_A_IN1, HIGH);
  digitalWrite(MOTOR_A_IN2, LOW);
  digitalWrite(MOTOR_B_IN1, HIGH);
  digitalWrite(MOTOR_B_IN2, LOW);
}

void moveForwardLeft() {
  digitalWrite(MOTOR_A_IN1, LOW);   // 左轮慢速或停止
  digitalWrite(MOTOR_A_IN2, LOW);
  digitalWrite(MOTOR_B_IN1, HIGH);  // 右轮快速
  digitalWrite(MOTOR_B_IN2, LOW);
}

void moveForwardRight() {
  digitalWrite(MOTOR_A_IN1, HIGH);  // 左轮快速
  digitalWrite(MOTOR_A_IN2, LOW);
  digitalWrite(MOTOR_B_IN1, LOW);   // 右轮慢速或停止
  digitalWrite(MOTOR_B_IN2, LOW);
}

void moveBackward(int duration = 0) {
  digitalWrite(MOTOR_A_IN1, LOW);
  digitalWrite(MOTOR_A_IN2, HIGH);
  digitalWrite(MOTOR_B_IN1, LOW);
  digitalWrite(MOTOR_B_IN2, HIGH);
  if (duration > 0) {
    delay(duration);
    stopMotors();
  }
}

void turnLeft(int duration = 0) {
  digitalWrite(MOTOR_A_IN1, LOW);
  digitalWrite(MOTOR_A_IN2, HIGH);
  digitalWrite(MOTOR_B_IN1, HIGH);
  digitalWrite(MOTOR_B_IN2, LOW);
  if (duration > 0) {
    delay(duration);
    stopMotors();
  }
}

void turnRight(int duration = 0) {
  digitalWrite(MOTOR_A_IN1, HIGH);
  digitalWrite(MOTOR_A_IN2, LOW);
  digitalWrite(MOTOR_B_IN1, LOW);
  digitalWrite(MOTOR_B_IN2, HIGH);
  if (duration > 0) {
    delay(duration);
    stopMotors();
  }
}

void rotateSlowly() {
  // 缓慢旋转搜索
  turnLeft(300);
  stopMotors();
  delay(200);
}

// 显示系统状态
void displayStatus() {
  static unsigned long lastDisplay = 0;
  if (millis() - lastDisplay > 1000) {
    Serial.printf("状态: %d, 信号强度: %d, 方向: %d, 障碍物: L%d F%d R%d\n",
                  currentState, irSignalStrength, signalDirection,
                  obstacles[0], obstacles[1], obstacles[2]);
    lastDisplay = millis();
  }
}

// 自动校准传感器
void autoCalibrate() {
  Serial.println("开始自动校准传感器...");
  Serial.println("请确保前方无障碍物，5秒后开始");
  delay(5000);
  
  for (int i = 0; i < 3; i++) {
    int baseValue = 0;
    for (int j = 0; j < 10; j++) {
      baseValue += readSensor(i);
      delay(50);
    }
    baseValue /= 10;
    thresholds[i] = baseValue * 0.7; // 阈值为基准值的70%
    Serial.printf("传感器%d基准值: %d, 阈值: %d\n", i, baseValue, thresholds[i]);
  }
  Serial.println("校准完成!");
}
